clc
clear

sigma_theta = 0.001;% 方位角测量误差
sigma_theta1 = 0.01;
sigma_fi = 0.001;% 高低角测量误差
sigma_s = 0.001; % 站址误差
S1 = [20 0 0];% 监测站1坐标
S2 = [-20 0 0];% 监测站2坐标
K = 100;% 正方形边长
% 辐射源坐标 XYZ
[X,Y] = meshgrid(-K:1:K);
X(101,:) = [];
Y(101,:) = [];
z = 10;
% 条件1
GDOP = zeros(2*K,2*K+1);
% 观测误差的协方差矩阵
dV_dVT = diag([sigma_theta^2 sigma_fi^2 sigma_theta1^2 sigma_fi^2]);
x1 = S1(1);y1 = S1(2);z1 = S1(3);
x2 = S2(1);y2 = S2(2);z2 = S2(3);
for i = 1:2*K
    for j = 1:2*K+1
        x = X(i,j);
        y = Y(i,j);
        % 计算单个gdop
        % 站1 
        theta1 = atan((x-x1)/(y-y1));
        r1 = sqrt((x-x1)^2+(y-y1)^2);
        fi1 = atan((z-z1)/r1);
        % 站2
        theta2 = atan((x-x2)/(y-y2));
        r2 = sqrt((x-x2)^2+(y-y2)^2);
        fi2 = atan((z-z2)/r2);
        % 系数矩阵
        F = [(cos(theta1))^2/(y-y1) sin(theta1)*cos(theta1)/(y1-y) 0;
            (x1-x)*cos(fi1)*sin(fi1)/(r1^2) (y1-y)*cos(fi1)*sin(fi1)/(r1^2) (cos(fi1))^2/r1
            (cos(theta2))^2/(y-y2) sin(theta2)*cos(theta2)/(y2-y) 0;
            (x2-x)*cos(fi2)*sin(fi2)/(r2^2) (y2-y)*cos(fi2)*sin(fi2)/(r2^2) (cos(fi2))^2/r2];
        h1 = [(cos(theta1))^2/(y1-y) sin(theta1)*cos(theta1)/(y-y1) 0;
            (x-x1)*cos(fi1)*sin(fi1)/(r1^2) (y-y1)*cos(fi1)*sin(fi1)/(r1^2) -(cos(fi1))^2/r1];
        h2 = [(cos(theta2))^2/(y2-y) sin(theta2)*cos(theta2)/(y-y2) 0;
            (x-x2)*cos(fi2)*sin(fi2)/(r2^2) (y-y2)*cos(fi2)*sin(fi2)/(r2^2) -(cos(fi2))^2/r2];
        % 布站误差的协方差矩阵
        dXs_dXsT = sigma_s^2*blkdiag(h1,h2)*blkdiag(h1,h2)';
        C = lsqminnorm(F'*F,F');
        % 定位误差的协方差矩阵
        P = C*(dV_dVT+dXs_dXsT)*C';
        GDOP(i,j) = sqrt(trace(P));
    end
end
% 作图
figure
contour(X,Y,GDOP,[0.001 0.01 0.03 0.05 0.1 0.2 0.4 1],'ShowText','on');
xlabel('x(km)');ylabel('y(km)');
title(['GDOP_m_i_n = ',num2str(min(min(GDOP)))]);